#pragma config(Sensor, in1,    armPot,              sensorPotentiometer)
#pragma config(Sensor, in2,    gyro,                sensorGyro)
#pragma config(Sensor, dgtl1,  area,                sensorDigitalIn)
#pragma config(Sensor, dgtl2,  color,               sensorDigitalIn)
#pragma config(Sensor, dgtl3,  ,                    sensorQuadEncoder)
#pragma config(Motor,  port1,           backleft,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           left,          tmotorNormal, openLoop)
#pragma config(Motor,  port3,           armright1,     tmotorNormal, openLoop)
#pragma config(Motor,  port4,           armleft1,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port5,           spin,          tmotorNormal, openLoop)
#pragma config(Motor,  port6,           spin2,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port7,           armleft2,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port8,           armright2,     tmotorNormal, openLoop)
#pragma config(Motor,  port9,           right,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          backright,     tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "PIDController.c"
/****************************************************************************************DRIVING FUNCTIONS*****************************************************************************************/
int leftDrivePower, rightDrivePower, x, y, rampLimit = 2, leftDrivePrevious, rightDrivePrevious;//drivetrain variables


void resetDriveVariables(){
  leftDrivePower = rightDrivePower = x = y = leftDrivePrevious = rightDrivePrevious = 0;
}
void setDriveMotors(){
  motor[left] = leftDrivePower;
  motor[backleft] = leftDrivePower;
  motor[right] = rightDrivePower;
  motor[backright] = rightDrivePower;
}

void stopDrive(){
  rightDrivePower = leftDrivePower = 0;
  setDriveMotors();
}

void driveArcade(int power, int turn, bool square, bool ramp){
  if(square)
  {
    if(power != 0)
    {
      y = ((power*power)/127)*(power/abs(power));
    }
    else
    {
      y = 0;
    }

    if(turn!=0)
    {
      x = ((turn*turn)/127)*(turn/abs(turn));
    }
    else
    {
      x = 0;
    }
  }
  else
  {
    y = power;
    x = turn;
  }

  leftDrivePower = y+x;
  rightDrivePower = y-x;

  if(ramp == true)
  {
    if(abs(rightDrivePower) > abs(rightDrivePrevious) + rampLimit)
    {
      if(rightDrivePower > (rightDrivePower + rampLimit))
      {
        rightDrivePower = rightDrivePrevious + rampLimit;
      }
      else
      {
        rightDrivePower = rightDrivePrevious - rampLimit;
      }
    }

    if(abs(leftDrivePower) > abs(leftDrivePrevious) + rampLimit)
    {
      if(leftDrivePower > (leftDrivePower + rampLimit))
      {
        leftDrivePower = leftDrivePrevious + rampLimit;
      }
      else
      {
        leftDrivePower = leftDrivePrevious - rampLimit;
      }
    }
  }

  setDriveMotors();

  leftDrivePrevious = leftDrivePower;
  rightDrivePrevious = rightDrivePower;
}
/*******************************************************************************************Arm & Spinner Code****************************************************************************************/

/*********PID Variables****************/
/**************************************/
PIDController Arm;

int ArmSetPoint;
int ArmKp = 10,ArmKi = 0, ArmKd = 0;

/**************************************/
/**************************************/

void setArmPids(kp, ki, kd){
  ArmKp = kp;
  ArmKi = ki;
  ArmKd = kd;
  setPIDs(Arm, ArmKp, ArmKi, ArmKd);
}

// LDR November 9, 2011 -- armPot: armFloor = 1936, armHighPlace = 3730, armMax =
int armPower, spinPower, armPosition, armTolerance, initialArmPosition, armOffset;//arm tolerance to be set
int armFloor = 1905, armLowPlace = 2670, armLowScore = 2570, armMidPlace = 3060, armMidScore = 2940, armHighPlace = 35252, armHighScore = 3525, armMax = 3845, armSetPoint, armState = 0;//arm positions. Place is position above goal, Score is on goal for tubes, floor is start position
int armInput, spinInput, armPreviousInput, armRampLimit = 2, rampSpinLimit = 0.2;//arm variables used in competitions code. Placed here to keep them with the rest of the arm variables
bool armPositionReached,  slowSpin = false;
int isolation;


void armMove(int aPower)
{
  armPower = aPower;
  motor[armleft1] = motor[armleft2] = motor[armright1] = motor[armright2] = armPower;
}

void spinMove(int sPower)
{
  spinPower = sPower;
  motor[spin] = motor[spin2] = spinPower;
}


/*****************************************************************************************Competition Code*****************************************************************************************/
/////////////////////////////////////////////////////////////////////////////////////////
//
//                          Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{

  //init(Arm,1);
}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task autonomous()
{


  bool interaction = (SensorValue[area] == 1); //user input switch to set if interaction zone or not (isolation)
  bool red = (SensorValue[color] == 1); //user input switch to set if on red team or not (blue)

  if (interaction) //if in interaction
  {
    if (red) //if on red team
    {

		 time1[T1] = 0;
		  while(time1[T1] < 1300) //flip out rotors and intake pre-load
			    spinMove(128);
			spinMove(0);

			time1[T1] = 0;
		  while(time1[T1] <1500)  // raise arm to high goal level
			    armMove(100);

		  time1[T1] = 0;
		  while (time1[T1] < 3000) //hold up arm to allow for loading
		      armMove(35);

		  time1[T1] = 0;
		  while (time1[T1] < 4500) //Drive forwards to goal, (while holding up arm)
		     driveArcade(40,0,false,false);

		  driveArcade(20,0,false,false);
		  time1[T1] = 0;
		  while (time1[T1] < 3000) //Release load and score!! (while holding up arm)
			  spinMove(-80);

		  armMove(0);
		  spinMove(0);
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//Drive home and lower arm
		  while (time1[T1]<1650){
		    if(time1[T1]>500){
		      armMove(-45);
		      if(SensorValue[armPot]<=armFloor+40) armMove(0);
		    }
		    driveArcade(-100,0,false,false);
		  }
		  armMove(0);
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//wait to repostition robot for 2 seconds
		  while(time1[T1]<2000){
		  }

		  time1[T1]=0;//Drive to the wall
		  spinMove(128);//because omnomnomnom
		  while(time1[T1]<2000){
		    driveArcade(40,0,false,false);

		  spinMove(0);

		  time1[T1]=0;//Drive home
		  while(time1[T1]<1000){
		    driveArcade(-99,0,false,false);
		  }
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//wait to repostition robot for 2 seconds
		  while(time1[T1]<2000){
		  }

		  while(SensorValue[armPot]<=armLowScore){
		    armMove(40)
		}
		armMove(0);
		driveArcade(0,0,false,false);



		}
	  }
	 else //if on blue team
	 {

			 time1[T1] = 0;
		  while(time1[T1] < 1300) //flip out rotors and intake pre-load
			    spinMove(128);
			spinMove(0);

			time1[T1] = 0;
		  while(time1[T1] <1500)  // raise arm to high goal level
			    armMove(100);

		  time1[T1] = 0;
		  while (time1[T1] < 3000) //hold up arm to allow for loading
		      armMove(35);

		  time1[T1] = 0;
		  while (time1[T1] < 4500) //Drive forwards to goal, (while holding up arm)
		     driveArcade(40,0,false,false);

		  driveArcade(20,0,false,false);
		  time1[T1] = 0;
		  while (time1[T1] < 3000) //Release load and score!! (while holding up arm)
			  spinMove(-80);

		  armMove(0);
		  spinMove(0);
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//Drive home and lower arm
		  while (time1[T1]<1650){
		    if(time1[T1]>500){
		      armMove(-45);
		      if(SensorValue[armPot]<=armFloor+40) armMove(0);
		    }
		    driveArcade(-100,0,false,false);
		  }
		  armMove(0);
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//wait to repostition robot for 2 seconds
		  while(time1[T1]<2000){
		  }

		  time1[T1]=0;//Drive to the wall
		  spinMove(128);//because omnomnomnom
		  while(time1[T1]<2000){
		    driveArcade(40,0,false,false);

		  spinMove(0);

		  time1[T1]=0;//Drive home
		  while(time1[T1]<1000){
		    driveArcade(-99,0,false,false);
		  }
		  driveArcade(0,0,false,false);

		  time1[T1]=0;//wait to repostition robot for 2 seconds
		  while(time1[T1]<2000){
		  }

		  while(SensorValue[armPot]<=armLowScore){
		    armMove(40)
		}
		armMove(0);
		driveArcade(0,0,false,false);

	}
   }
 }
 else //if in isolation
 {
   if (red) //if on the red team
   {
     time1[T1] = 0;
		  while(time1[T1] < 1200) //flip out rotors and intake pre-load
			    spinMove(128);
			spinMove(0);

			time1[T1] = 0;
		  while(time1[T1] <1500)  // raise arm to high goal level
			    armMove(100);

			 armMove(50); //hold up arm

		  time1[T1] = 0;
		  while (time1[T1] < 1000) //Drive forwards to goal. (while holding up arm)
		     driveArcade(40,0,false,false);

		  time1[T1] = 0;
		  while (time1[T1] < 1500) //Release load and score!! (while holding up the arm)
			  spinMove(-80);

		  armMove(0);
		  spinMove(0);
   }
   else //if one the blue team
   {
     time1[T1] = 0;
		  while(time1[T1] < 1200) //flip out rotors and intake pre-load
			    spinMove(128);
			spinMove(0);

			time1[T1] = 0;
		  while(time1[T1] <1500)  // raise arm to high goal level
			    armMove(100);

			armMove(50); // hold up arm

		  time1[T1] = 0;
		  while (time1[T1] < 1000) //Drive forwards to goal. (while holding up arm)
		     driveArcade(40,0,false,false);

		  time1[T1] = 0;
		  while (time1[T1] < 1000) //Release load and score!! (while holding up arm)
			  spinMove(-80);

		  armMove(0);
		  spinMove(0);
   }

 }


}
/////////////////////////////////////////////////////////////////////////////////////////
//
//                                 User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////

task usercontrol()
{
	// User control code here, inside the loop

resetDriveVariables();
  armState = 0;
	  // This is the main execution loop for the user control program. Each time through the loop
	  // your program should update motor + servo values based on feedback from the joysticks.
	while (true)
	{
    driveArcade(vexRT[Ch3],vexRT[Ch4],true,true);//leave both square and ramp as false until the code is proven to work

    if (vexRT[Btn6U] != 0){
      spinInput = 128;
      slowSpin = false;
    }
    else if(vexRT[Btn6D] != 0)
    {
      spinInput = -128;
      slowSpin = false;
    }
    else if(vexRT[Btn5U] != 0)
    {
      spinInput = 40;
      slowSpin = true;
    }
    else if(vexRT[Btn5D]){
      spinInput = -31;
      slowSpin = true;
    }
    else
    {
      spinInput = 0;
      slowSpin = false;
    }

    armMove(vexRT[Ch2]);
    spinMove(spinInput);

  }
}
